#include "BSP_MOTO.h"
#include "BSP_ENCODER.h"
#include "tim.h"

struct MOTO_INF
{
	int speed,last_speed;
	int Target_speed;
	int pwm_value;
	int kp,ki,err,sum_err;
};


int pwm_maxlimit=999;
int pwm_minlimit=30;
int ki_limit=5;	


int kp = 20;
int ki = 30;
int kd = -10;

struct MOTO_INF M1,M2,M3,M4;

void MOTO_INIT(void)
{
	HAL_TIM_PWM_Start(&MOTO_TIM_SOURSE,MOTO1_PWM_CH);
	HAL_TIM_PWM_Start(&MOTO_TIM_SOURSE,MOTO2_PWM_CH);
	HAL_TIM_PWM_Start(&MOTO_TIM_SOURSE,MOTO3_PWM_CH);
	HAL_TIM_PWM_Start(&MOTO_TIM_SOURSE,MOTO4_PWM_CH);
	
	HAL_GPIO_WritePin(MOTO1_IN1_GPIO_Port,MOTO1_IN1_Pin,0); 
	HAL_GPIO_WritePin(MOTO1_IN2_GPIO_Port,MOTO1_IN2_Pin,0);
	
	HAL_GPIO_WritePin(MOTO2_IN1_GPIO_Port,MOTO2_IN1_Pin,0); 
	HAL_GPIO_WritePin(MOTO2_IN2_GPIO_Port,MOTO2_IN2_Pin,0);
	
	HAL_GPIO_WritePin(MOTO3_IN1_GPIO_Port,MOTO3_IN1_Pin,0); 
	HAL_GPIO_WritePin(MOTO3_IN2_GPIO_Port,MOTO3_IN2_Pin,0);
	
	HAL_GPIO_WritePin(MOTO4_IN1_GPIO_Port,MOTO4_IN1_Pin,0); 
	HAL_GPIO_WritePin(MOTO4_IN2_GPIO_Port,MOTO4_IN2_Pin,0);
	
	HAL_GPIO_WritePin(MOTO1_2_STBY_GPIO_Port,MOTO1_2_STBY_Pin,1);
	HAL_GPIO_WritePin(MOTO3_4_STBY_GPIO_Port,MOTO3_4_STBY_Pin,1);
	
	M1.Target_speed = 0;
	M1.pwm_value = 0;
	M1.sum_err = 0;
	M1.kp = kp;
	M1.ki=ki;
	
	M2.Target_speed = 0;
	M2.pwm_value = 0;
	M2.sum_err = 0;
	M2.kp = kp;
	M2.ki=ki;
	
	M3.Target_speed = 0;
	M3.pwm_value = 0;
	M3.sum_err = 0;
	M3.kp = kp;
	M3.ki=ki;
	
	M4.Target_speed = 0;
	M4.pwm_value = 0;
	M4.sum_err = 0;
	M4.kp = kp;
	M4.ki=ki;
}
void MOTO1_PWM_SET(int value)
{
	if(value < 0)
	{
		value = ~value+1;
		HAL_GPIO_WritePin(MOTO1_IN1_GPIO_Port,MOTO1_IN1_Pin,0); 
		HAL_GPIO_WritePin(MOTO1_IN2_GPIO_Port,MOTO1_IN2_Pin,1);
	}
	else
	{
		HAL_GPIO_WritePin(MOTO1_IN1_GPIO_Port,MOTO1_IN1_Pin,1); 
		HAL_GPIO_WritePin(MOTO1_IN2_GPIO_Port,MOTO1_IN2_Pin,0);
	}
	__HAL_TIM_SetCompare(&MOTO_TIM_SOURSE,MOTO1_PWM_CH,value);
}

void MOTO2_PWM_SET(int value)
{
	if(value < 0)
	{
		value = ~value+1;
		HAL_GPIO_WritePin(MOTO2_IN1_GPIO_Port,MOTO2_IN1_Pin,1); 
		HAL_GPIO_WritePin(MOTO2_IN2_GPIO_Port,MOTO2_IN2_Pin,0);
	}
	else
	{
		HAL_GPIO_WritePin(MOTO2_IN1_GPIO_Port,MOTO2_IN1_Pin,0); 
		HAL_GPIO_WritePin(MOTO2_IN2_GPIO_Port,MOTO2_IN2_Pin,1);
	}
	__HAL_TIM_SetCompare(&MOTO_TIM_SOURSE,MOTO2_PWM_CH,value);
}

void MOTO3_PWM_SET(int value)
{
	if(value < 0)
	{
		value = ~value+1;
		HAL_GPIO_WritePin(MOTO3_IN1_GPIO_Port,MOTO3_IN1_Pin,0); 
		HAL_GPIO_WritePin(MOTO3_IN2_GPIO_Port,MOTO3_IN2_Pin,1);
	}
	else
	{
		HAL_GPIO_WritePin(MOTO3_IN1_GPIO_Port,MOTO3_IN1_Pin,1); 
		HAL_GPIO_WritePin(MOTO3_IN2_GPIO_Port,MOTO3_IN2_Pin,0);
	}
	__HAL_TIM_SetCompare(&MOTO_TIM_SOURSE,MOTO3_PWM_CH,value);
}

void MOTO4_PWM_SET(int value)
{
	if(value < 0)
	{
		value = ~value+1;
		HAL_GPIO_WritePin(MOTO4_IN1_GPIO_Port,MOTO4_IN1_Pin,1); 
		HAL_GPIO_WritePin(MOTO4_IN2_GPIO_Port,MOTO4_IN2_Pin,0);
	}
	else
	{
		HAL_GPIO_WritePin(MOTO4_IN1_GPIO_Port,MOTO4_IN1_Pin,0); 
		HAL_GPIO_WritePin(MOTO4_IN2_GPIO_Port,MOTO4_IN2_Pin,1);
	}
	__HAL_TIM_SetCompare(&MOTO_TIM_SOURSE,MOTO4_PWM_CH,value);
}



void MOTO1_SPEED_SET(int Target_speed,int speed)
{
	M1.Target_speed = Target_speed;
	
	M1.speed = speed;
	M1.err = M1.Target_speed-M1.speed;
		
	M1.pwm_value += (int)(M1.err*M1.kp/10
		+M1.sum_err*M1.ki/1000+
		(M1.speed-M1.last_speed)*kd);
	
	if(M1.pwm_value>pwm_maxlimit)M1.pwm_value=pwm_maxlimit;
	else if(M1.pwm_value<-pwm_maxlimit)M1.pwm_value=-pwm_maxlimit;

	
	if(M1.sum_err>ki_limit)M1.sum_err=ki_limit;
	else if(M1.sum_err<-ki_limit)M1.sum_err=-ki_limit;
	
	M1.last_speed = M1.speed;
	M1.sum_err += M1.err;
	if(M1.pwm_value>10 && M1.pwm_value<50)
	{
		MOTO1_PWM_SET(M1.pwm_value+pwm_minlimit);
	}
	else if(M1.pwm_value<-10 && M1.pwm_value>-50)
	{
		MOTO1_PWM_SET(M1.pwm_value-pwm_minlimit);
	}
	else MOTO1_PWM_SET(M1.pwm_value);
//	printf("M1:%d\r\n",M1.speed);
//	printf("pwm_value:%d\r\n",M1.pwm_value);
}

void MOTO2_SPEED_SET(int Target_speed,int speed)
{
	M2.Target_speed = Target_speed;
	
	M2.speed = speed;
	M2.err = M2.Target_speed-M2.speed;
		
	M2.pwm_value += (int)(M2.err*M2.kp/10
		+M2.sum_err*M2.ki/1000+
		(M2.speed-M2.last_speed)*kd);
	
	if(M2.pwm_value>pwm_maxlimit)M2.pwm_value=pwm_maxlimit;
	else if(M2.pwm_value<-pwm_maxlimit)M2.pwm_value=-pwm_maxlimit;

	
	if(M2.sum_err>ki_limit)M2.sum_err=ki_limit;
	else if(M2.sum_err<-ki_limit)M2.sum_err=-ki_limit;
	M2.last_speed = M2.speed;
	M2.sum_err += M2.err;
	if(M2.pwm_value>10 && M2.pwm_value<50)
	{
		MOTO2_PWM_SET(M2.pwm_value+pwm_minlimit);
	}
	else if(M2.pwm_value<-10 && M2.pwm_value>-50)
	{
		MOTO2_PWM_SET(M2.pwm_value-pwm_minlimit);
	}
	else MOTO2_PWM_SET(M2.pwm_value);
//	printf("M2:%d\r\n",M2.speed);
//	printf("pwm_value:%d\r\n",M2.pwm_value);
}

void MOTO3_SPEED_SET(int Target_speed,int speed)
{
	M3.Target_speed = Target_speed;
	
	M3.speed = speed;
	M3.err = M3.Target_speed-M3.speed;
		
	M3.pwm_value += (int)(M3.err*M3.kp/10
		+M3.sum_err*M3.ki/1000+
		(M3.speed-M3.last_speed)*kd);
	
	if(M3.pwm_value>pwm_maxlimit)M3.pwm_value=pwm_maxlimit;
	else if(M3.pwm_value<-pwm_maxlimit)M3.pwm_value=-pwm_maxlimit;

	
	
	if(M3.sum_err>ki_limit)M3.sum_err=ki_limit;
	else if(M3.sum_err<-ki_limit)M3.sum_err=-ki_limit;
	
	M3.last_speed = M3.speed;
	M3.sum_err += M3.err;
	if(M3.pwm_value>10 && M3.pwm_value<50)
	{
		MOTO3_PWM_SET(M3.pwm_value+pwm_minlimit);
	}
	else if(M3.pwm_value<-10 && M3.pwm_value>-50)
	{
		MOTO3_PWM_SET(M3.pwm_value-pwm_minlimit);
	}
	else MOTO3_PWM_SET(M3.pwm_value);
//	printf("M3:%d\r\n",M3.speed);
//	printf("pwm_value:%d\r\n",M3.pwm_value);
}


void MOTO4_SPEED_SET(int Target_speed,int speed)
{
	M4.Target_speed = Target_speed;
	
	M4.speed = speed;
	M4.err = M4.Target_speed-M4.speed;
		
	M4.pwm_value += (int)(M4.err*M4.kp/10
		+M4.sum_err*M4.ki/1000+
		(M4.speed-M4.last_speed)*kd);
	
	if(M4.pwm_value>pwm_maxlimit)M4.pwm_value=pwm_maxlimit;
	else if(M4.pwm_value<-pwm_maxlimit)M4.pwm_value=-pwm_maxlimit;
	
	
	
	if(M4.sum_err>ki_limit)M4.sum_err=ki_limit;
	else if(M4.sum_err<-ki_limit)M4.sum_err=-ki_limit;
	
	M4.last_speed = M4.speed;
	M4.sum_err += M4.err;
	if(M4.pwm_value>10 && M4.pwm_value<50)
	{
		MOTO4_PWM_SET(M4.pwm_value+pwm_minlimit);
	}
	else if(M4.pwm_value<-10 && M4.pwm_value>-50)
	{
		MOTO4_PWM_SET(M4.pwm_value-pwm_minlimit);
	}
	else MOTO4_PWM_SET(M4.pwm_value);
//	printf("M4:%d\r\n",M4.speed);
//	printf("pwm_value:%d\r\n",M4.pwm_value);
}

void clean_sumerr()
{
	M1.sum_err = 0;
	M2.sum_err = 0;
	M3.sum_err = 0;
	M4.sum_err = 0;
	M1.pwm_value = 0;
	M2.pwm_value = 0;
	M3.pwm_value = 0;
	M4.pwm_value = 0;
}






